/**
  ******************************************************************************
  * @file           : XYXKC_MechDog.cpp
  * @author         : admin
  * @brief          : None
  * @attention      : None
  * @date           : 2024/11/14
  ******************************************************************************
  */

#include "XYXKC_MechDog.h"

XYXKC_MechDog::XYXKC_MechDog(uint8_t rx, uint8_t tx) {
    m_serial = new SoftwareSerial(rx, tx);
}

XYXKC_MechDog::~XYXKC_MechDog(){
    m_serial = NULL;
}
void XYXKC_MechDog::begin() {
    m_serial->begin(9600);
}

void XYXKC_MechDog::runningAction(int actionNumber) {
    m_serial->println("CMD|2|1|"+String(actionNumber)+"|$");
}
void XYXKC_MechDog::motion(int motionNumber) {
    m_serial->println("CMD|3|"+String(motionNumber)+"|$");
}
void XYXKC_MechDog::ultrasonicObstacleAvoidance(int boolNumber) {
    m_serial->println("CMD|4|2|"+String(boolNumber)+"|$");
}
// 分割字符串的函数定义
void splitString(String input, char delimiter, String tokens[]) {
    int tokenCount = 0;
    int startIndex = 0;
    int endIndex;

    while ((endIndex = input.indexOf(delimiter, startIndex)) != -1) {
        tokens[tokenCount] = input.substring(startIndex, endIndex);
        tokenCount++;
        startIndex = endIndex + 1;

        if (startIndex < input.length()) {
            tokens[tokenCount] = input.substring(startIndex);
            tokenCount++;
        }
    }
}


int XYXKC_MechDog::getUltrasonic() {

    m_serial->println("CMD|4|1|$");
    delay(100);
    String start="";
    while (m_serial->available()) {
        start += (char)m_serial->read();
    }
    String tokens[4];  // 创建一个String数组来存储分割后的字符串
    splitString(start, '|', tokens);

    if (tokens[0] == "CMD" && tokens[1] == "4" && tokens[3] == "$") {
        return tokens[2].toInt();
    }


    return 0;
}

int XYXKC_MechDog::getElectricityLevel() {

    m_serial->println("CMD|6|$");
    delay(100);
    String start="";
    while (m_serial->available()) {
        start += (char)m_serial->read();
    }
    String tokens[4];  // 创建一个String数组来存储分割后的字符串
    splitString(start, '|', tokens);

    if (tokens[0] == "CMD" && tokens[1] == "6" && tokens[3] == "$") {
        return tokens[2].toInt();
    }


    return 999999;
}